#include "cw_control.h"


MotorDef LeftMT,RightMT;
TrackDef Track;
PhyparamDef PhyParam;
MCUparamDef MCUParam;
int lsendspd;
int rsendspd;

void MCUParamInit(void)
{
	MCUParam.pwmvalmax=1000;
	MCUParam.pwmvalprt=1000;
}

void PhyParamInit(void)
{
	PhyParam.PulsePerMeter=PPM;
	PhyParam.wheelBase =WLB;     	// 单位m
	PhyParam.wheelR = WLR; 					// 单位m
	PhyParam.wheelC = WLC;						// 单位m
	
}

//控制初始化
void control_init(void)
{
	//参数初始化
	MCUParamInit();
	PhyParamInit();
	PID_Init();
	//控制初始化 PWMCTRL|SPDCTRL|POSCTRL|MODCTRL|TCKCTRL
	LeftMT.flag=RightMT.flag=(PWMCTRL|SPDCTRL|POSCTRL|MODCTRL|TCKCTRL);
	//LeftMT.flag=RightMT.flag=PWMCTRL|SPDCTRL;
	//LeftMT.flag=RightMT.flag=PWMCTRL;
	LeftMT.status=RightMT.status=FREESTA;
	//开启PWM
	PWM_START();
	//使能STBY引脚
	STBY_SET();
}

//设置PWM
void set_pwm(int leftval,int rightval)
{
	if(((LeftMT.flag&PWMCTRL)==0)||(RightMT.flag&PWMCTRL)==0)
	{
		return;
	}
	//左电机控制
	if(leftval>=0)//正转
	{
		LeftMT.DIR=FWD;
		if(leftval>=MCUParam.pwmvalprt)
		{
			leftval=MCUParam.pwmvalprt;
		}
		LeftMT.PWMval=leftval;
		BIN1_CLR();
		BIN2_SET();
	}
	else //反转
	{
		LeftMT.DIR=REV;
		if(leftval<=-MCUParam.pwmvalprt)
		{
			leftval=-MCUParam.pwmvalprt;
		}
		LeftMT.PWMval=leftval;
		BIN1_SET();
		BIN2_CLR();
	}
	//右电机控制
	if(rightval>=0)//正转
	{
		RightMT.DIR=FWD;
		if(rightval>=MCUParam.pwmvalprt)
		{
			rightval=MCUParam.pwmvalprt;
		}
		RightMT.PWMval=rightval;
		AIN1_CLR();
		AIN2_SET();
	}
	else //反转
	{
		RightMT.DIR=REV;
		if(rightval<=-MCUParam.pwmvalprt)
		{
			rightval=-MCUParam.pwmvalprt;
		}
		RightMT.PWMval=rightval;
		AIN1_SET();
		AIN2_CLR();
	}
	//修改PWM占空比
	PWM_SET(intabs(LeftMT.PWMval),intabs(RightMT.PWMval));
}

//读取速度
BfloatDef get_speed(SpeedUnitDef unit)
{
	BfloatDef temp={-1,-1};
	temp.leftval=speed_turn(LeftMT.PVreality*100,PPS,unit);
	temp.rightval=speed_turn(RightMT.PVreality*100,PPS,unit);
	return temp;
}

//设置速度
void set_speed(int leftval,int rightval,SpeedUnitDef unit)
{
	leftval = speed_turn(leftval,unit,PPS);
	rightval = speed_turn(rightval,unit,PPS);
	LeftMT.PVtarget=leftval;
	RightMT.PVtarget=rightval;
}

//读取路程
BfloatDef get_postition(PostitionUnitDef unit)
{
	BfloatDef temp={-1,-1};
	temp.leftval=postition_turn(LeftMT.PPreality,PLE,unit);
	temp.rightval=postition_turn(RightMT.PPreality,PLE,unit);
	return temp;
}

//设置路程
void set_postition(long leftval,long rightval,PostitionUnitDef unit)
{
	clear_postition();
	leftval=postition_turn(leftval,unit,PLE);
	rightval=postition_turn(rightval,unit,PLE);
	LeftMT.PPtarget=leftval;
	RightMT.PPtarget=rightval;
}

//读取剩余路程
BfloatDef get_rest_postition(PostitionUnitDef unit)
{
	BfloatDef temp={-1,-1};
	if(LeftMT.PPtarget<0)
	{
		temp.leftval=postition_turn(LeftMT.PPreality-LeftMT.PPtarget,PLE,unit);
	}
	else
	{
		temp.leftval=postition_turn(LeftMT.PPtarget-LeftMT.PPreality,PLE,unit);
	}
	if(RightMT.PPtarget<0)
	{
		temp.rightval=postition_turn(RightMT.PPreality-RightMT.PPtarget,PLE,unit);
	}
	else
	{
		temp.rightval=postition_turn(RightMT.PPtarget-RightMT.PPreality,PLE,unit);
	}
	return temp;
}

//清空路程
void clear_postition(void)
{
	LeftMT.PPreality=RightMT.PPreality=0;
}

void setPVexcept(float speedmps)
{
		LeftMT.PVexcept=RightMT.PVexcept=speed_turn(speedmps,MPS,PPS);
}

void LineAmend(int err,PIDDef* hPID)
{
	//得到正数小车往左调
	if(err>0)//小车偏右右边修正加速
	{
		RightMT.PVtarget+=Position_PID(-err, 0, hPID);
		//LeftMT.PVtarget=0;
	}
	else if(err<=0)//小车偏左左边修正加速
	{
		LeftMT.PVtarget+=Position_PID(err, 0, hPID);
		//RightMT.PVtarget=0;
	}
}

//循迹控制
void track_control(void)
{
	if((LeftMT.flag&TCKCTRL)&&(RightMT.flag&TCKCTRL)&&Track.ready==true)
	{
		OLED_ShowNum(0,1,100+LeftMT.flag,4,12);
		if(Track.mode!=0)
		{
			LeftMT.PVtarget=0;
			RightMT.PVtarget=0;
		}
		else
		{
			if(Track.exceptspeed>0)
			{
				setPVexcept(Track.exceptspeed);
			}
			RightMT.PVtarget=LeftMT.PVtarget=LeftMT.PVexcept;
			LineAmend(-Track.err,&TlinePID);
			//Track.err=0;
		}

	}
}

//速度控制
void speed_control(void)
{
	if((LeftMT.flag&SPDCTRL)&&(RightMT.flag&SPDCTRL))
	{
		//调用左轮速度PID运算函数
		LeftMT.PWMval += Position_PID(LeftMT.PVreality,LeftMT.PVtarget,&LspdPID);
		//调用右轮速度PID运算函数
		RightMT.PWMval += Position_PID(RightMT.PVreality,RightMT.PVtarget,&RspdPID);
	}
	if((LeftMT.flag&PWMCTRL)&&(RightMT.flag&PWMCTRL))
	{
		set_pwm(LeftMT.PWMval, RightMT.PWMval);
	}
}

int cspdl=0;
int cspdr=0;
//位置控制
void postition_control(void)
{
	if((LeftMT.flag&POSCTRL)&&(RightMT.flag&POSCTRL))
	{
		if(longabs(LeftMT.PPtarget-LeftMT.PPreality)<cspdl*4)
		{
			LeftMT.PVtarget=0;
		}
		else if(longabs(LeftMT.PPtarget-LeftMT.PPreality)<cspdl*10)
		{
			LeftMT.PVtarget=LeftMT.PVtarget/2;
		}
		else
		{
			cspdl=longabs(LeftMT.PVtarget);
		}
		if(longabs(RightMT.PPtarget-RightMT.PPreality)<cspdr*4)
		{
			RightMT.PVtarget=0;
		}
		else if(longabs(RightMT.PPtarget-RightMT.PPreality)<cspdr*10)
		{
			RightMT.PVtarget=RightMT.PVtarget/2;
		}
		else
		{
			cspdr=longabs(RightMT.PVtarget);
		}
	}
}

//速度转换
float speed_turn(float oldspeed,SpeedUnitDef oldunit,SpeedUnitDef newunit)
{
	if(oldunit==PPS)
	{
		oldspeed=oldspeed*100;
		if(newunit==MPS)
		{
			return oldspeed/PhyParam.PulsePerMeter*1.2;
		}
		return oldspeed;
	}
	else if(oldunit==MPS)
	{
		if(newunit==PPS)
		{
			return (oldspeed*0.83333f*PhyParam.PulsePerMeter)/100;
		}
		return oldspeed;
	}
	else
		return oldspeed;
}

//位移转换
double postition_turn(double olddistance,PostitionUnitDef oldunit,PostitionUnitDef newunit)
{
	if(oldunit==PLE)
	{
		if(newunit==MTR)
		{
			return olddistance/PhyParam.PulsePerMeter; 
		}
		return olddistance;
	}
	else if(oldunit==MTR)
	{
		if(newunit==PLE)
		{
			//if(LeftMT.PVtarget&&RightMT.PVtarget)return olddistance*((5.42*(RightMT.PVtarget+LeftMT.PVtarget)/2)+PhyParam.PulsePerMeter-100);
			return olddistance*PhyParam.PulsePerMeter;
		}
		return olddistance;
	}
	else
		return olddistance;
}

void motor_status_update(void)
{
	//运行检测
	if((LeftMT.flag&PWMCTRL)&&(RightMT.flag&PWMCTRL))
	{
		if(intabs(LeftMT.PVreality)>2)
		{
			LeftMT.status|=RSPDSTA;
		}
		else
		{
			LeftMT.status&=~RSPDSTA;
		}
		if(intabs(RightMT.PVreality)>2)
		{
			RightMT.status|=RSPDSTA;
		}
		else
		{
			RightMT.status&=~RSPDSTA;
		}
	}
	//速度检测
	if((LeftMT.flag&SPDCTRL)&&(RightMT.flag&SPDCTRL))
	{
		if(intabs(LeftMT.PVtarget-LeftMT.PVreality)>1)
		{
			LeftMT.status|=SPEDSTA;
		}
		else
		{
			LeftMT.status&=~SPEDSTA;
		}
		if(intabs(RightMT.PVtarget-RightMT.PVreality)>1)
		{
			RightMT.status|=SPEDSTA;
		}
		else
		{
			RightMT.status&=~SPEDSTA;
		}
	}
	//路程检测
	if((LeftMT.flag&POSCTRL)&&(RightMT.flag&POSCTRL))
	{
		if(longabs(LeftMT.PPtarget-LeftMT.PPreality)>100)
		{
			LeftMT.status|=POSTSTA;
		}
		else
		{
			LeftMT.status&=~POSTSTA;
		}
		if(longabs(RightMT.PPtarget-RightMT.PPreality)>100)
		{
			RightMT.status|=POSTSTA;
		}
		else
		{
			RightMT.status&=~POSTSTA;
		}
	}
	//巡线检测
	if((LeftMT.flag&TCKCTRL)&&(RightMT.flag&TCKCTRL))
	{
		if(intabs(Track.err)>1)
		{
			LeftMT.status|=TCKLSTA;
		}
		else
		{
			LeftMT.status&=~TCKLSTA;
		}
	}
}

void spinto(float leftspeed,float rightspeed,SpeedUnitDef spdunit,double leftpostition,double rightpostition,PostitionUnitDef posunit)
{
	CtrlFlagDef tempflag = GET_CTRLFLAG();
	clear_postition();
	LeftMT.flag=RightMT.flag=0x00;
	LeftMT.PVtarget=speed_turn(leftspeed,spdunit,PPS);
	RightMT.PVtarget=speed_turn(rightspeed,spdunit,PPS);
	LeftMT.PPtarget=postition_turn(leftpostition,posunit,PLE);
	RightMT.PPtarget=postition_turn(rightpostition,posunit,PLE);
	SET_CTRLFLAG(tempflag);
}


void spinfor(float leftspeed,float rightspeed,SpeedUnitDef spdunit)
{
	CtrlFlagDef tempflag = LeftMT.flag;
	LeftMT.flag=RightMT.flag=0x00;
	LeftMT.PVtarget=speed_turn(leftspeed,spdunit,PPS);
	RightMT.PVtarget=speed_turn(rightspeed,spdunit,PPS);
	LeftMT.flag=RightMT.flag=(tempflag&(~POSCTRL));
}

//路程阻塞函数
//一般为waitposfor(50,PLE,0)
int waitposfor(double diearea,PostitionUnitDef unit,int spdclrflag)
{
	BfloatDef temp;
	while(1)
	{
		temp = get_rest_postition(unit);
		delayus(1);//防止死循环被优化
		if(temp.leftval<diearea&&temp.rightval<diearea)
		{
			setPVexcept(0);
			clear_postition();
			LeftMT.PPtarget=RightMT.PPtarget=0;
			if(spdclrflag)
			{
				set_speed(0,0,PPS);
			}
			return 1;
		}
		
	}
}

/**
 * @brief 两轮差速模型
 *
 * 以一定的参数进行差速行走，实现转弯和画弧
 *
 * @param leftMT(motor) 左电机对象
 * @param rightMT(motor) 右电机对象
 * @param velocity(double) 车驱动轮轴中心速度（mps）
 * @param omega(float) 车驱动轮轴角速度(rad/s)
 * @param distance(long) mega!=0-->代表旋转角度 正负决定旋转方向 >0 : 逆时针 <0 顺时针 缺省值: 0 一直转，  方向由速度velocity决定
 *											 mega=0-->代表行进距离（m） 正负决定行进方向 >0 : 正向 <0 逆向 缺省值: 0 一直移动，方向由速度velocity决定
 *
 * @return (int)-2 ： 向后前行一定距离 -1 ：后向旋转一定角度 0:一直转  1 ：前向旋转一定角度 2 ：向前前行一定距离
 */
int modelctrl(float velocity, float omega,double distance,uint8_t choke)
{
	//PWMCTRL|SPDCTRL|POSCTRL|MODCTRL|TCKCTRL
	if(!(LeftMT.flag&MODCTRL)&&(RightMT.flag&MODCTRL))//未开启差速控制，直接返回
	{
		return 255;
	}
	CtrlFlagDef tempflag = GET_CTRLFLAG();
	SET_CTRLFLAG(0x00);
	if(velocity==0)//零转
	{
		if(omega==0)//没有任何速度，返回
			return 0;
		else
		{
			if(distance==0)//一直零转
			{
				float leftspeed = omega * PhyParam.wheelBase / 2;
				float rightspeed = -omega * PhyParam.wheelBase/ 2;
				spinfor(leftspeed,rightspeed,MPS);
				SET_CTRLFLAG(tempflag&~POSCTRL);
				if(choke)waitposfor(50,PLE,0);
				return doublesgn(omega)*1;
			}
			else					//零转一定角度
			{
				distance*=1.12;//修正系数
				omega = doublesgn(distance)*doubleabs(omega);
				double d = doubleabs((PhyParam.wheelBase/ 2)*( 3.14159 * distance / 180));
				float leftspeed = omega * PhyParam.wheelBase / 2;
				float rightspeed = -omega * PhyParam.wheelBase/ 2;
				spinto(leftspeed,rightspeed,MPS,doublesgn(leftspeed)*d,doublesgn(rightspeed)*d,MTR);
				SET_CTRLFLAG(tempflag|POSCTRL);
				if(choke)waitposfor(50,PLE,0);
				return doublesgn(distance)*2;
			}
		}
	}
	else//差转
	{
			if(omega==0)
			{
				if(distance==0)			//向前或向后一直直行
				{
					setPVexcept(velocity);
					spinfor(velocity,velocity,MPS);
					SET_CTRLFLAG(tempflag&~POSCTRL);
					if(choke)waitposfor(50,PLE,0);
					return doublesgn(velocity)*3;
				}
				else
				{
					float spd = doublesgn(distance)*floatabs(velocity);
					setPVexcept(spd);
					spinto(spd,spd,MPS,distance,distance,MTR);
					SET_CTRLFLAG(tempflag|POSCTRL);
					if(choke)waitposfor(50,PLE,0);
					return doublesgn(distance)*4;
				}
			}
			else 
			{
				if(distance==0)			//向前或向后一直转向
				{
					float leftspeed = velocity + omega * PhyParam.wheelBase / 2;
					float rightspeed = velocity - omega * PhyParam.wheelBase/ 2;
					spinfor(leftspeed,rightspeed,MPS);
					SET_CTRLFLAG(tempflag&~POSCTRL);
					if(choke)waitposfor(50,PLE,0);
					return doublesgn(omega)*5;
				}
				else								//向前或向后转向一定角度
				{
					distance*=1.12;//修正系数
					velocity = floatabs(velocity);
					float leftspeed = velocity + omega * PhyParam.wheelBase / 2;
					float rightspeed = velocity - omega * PhyParam.wheelBase/ 2;
					float minispeed =  doublemin(floatabs(leftspeed),floatabs(rightspeed));//得到内边速度
					//计算内半径
					float inr = minispeed/floatabs(omega);
					//计算外半径
					float outr = inr + PhyParam.wheelBase;
					//计算内半径移动长度
					double ind = inr *( 3.14159 * distance / 180);
					//计算外半径移动长度
					double outd = outr *( 3.14159 * distance / 180);
					if(distance>0)
					{
						if (leftspeed > rightspeed) // 右前转
							spinto(leftspeed,rightspeed,MPS,outd,ind,MTR);
						else											  // 左前转
							spinto(leftspeed,rightspeed,MPS,ind,outd,MTR);
					}
					else
					{
						if (leftspeed > rightspeed) // 右后转
							spinto(-leftspeed,-rightspeed,MPS,outd,ind,MTR);
						else											  // 左后转
							spinto(-leftspeed,-rightspeed,MPS,ind,outd,MTR);
						
					}
					SET_CTRLFLAG(tempflag|POSCTRL);
					if(choke)waitposfor(50,PLE,0);
					return doublesgn(distance)*6;
				}	
			}
	}
}